!***********************************************************************

! Code converted using TO_F90 by Alan Miller
! Date: 2013-05-26  Time: 22:02:34

!************************ CADAC UTILITIES ******************************
!***************************** UTL.FOR *********************************
!***********************************************************************
!***  *
!***  * MATxxx, VECxxx, TABLx,and CADxxx Utility Subroutines.
!***  *
!***  * MODIFICATION HISTORY
!***  * 970430 Modified VECANG to prevent rare math overflow problem, PZi
!***  * 970501 Version 2.0, Released with CADX2.FOR, Version 2
!***  * 970619 Corrections in MATADD and MATSUB, PZi
!***  * 980529 RANF Function added for RANF calls with FNGAUSS routine, bc
!***  * 991122 Added new table look-up TBLPx routines with pointers, PZi
!***  * 000204 Added subroutine US76 std atmosphere, PZi
!***  * 000412 Included CADxx utilities from UTL8.FOR, renamed UTL3.FOR, PZi
!***  *
!***  *****************************************************************

SUBROUTINE matadd (c,a,b,n1,n2)

!*** C(N1*N2) IS OBTAINED FROM ADDING A(N1*N2) TO B(N1*N2)

!970619 K=N2*(J-1)+I relaced by  K=N1*(J-1)+I



REAL, INTENT(OUT)                        :: c(1)
REAL, INTENT(IN)                         :: a(1)
REAL, INTENT(IN)                         :: b(1)
INTEGER, INTENT(IN)                      :: n1
INTEGER, INTENT(IN)                      :: n2


DO  j=1,n2
  DO  i=1,n1
    k=n1*(j-1)+i
    c(k)=a(k)+b(k)
  END DO
END DO
RETURN
END SUBROUTINE matadd

SUBROUTINE matsub (c,a,b,n1,n2)

!*** C(N1*N2) IS OBTAINED FROM SUBSTRACTING B(N1*N2) FROM A(N1*N2)

!970619 K=N2*(J-1)+I relaced by  K=N1*(J-1)+I




REAL, INTENT(OUT)                        :: c(1)
REAL, INTENT(IN)                         :: a(1)
REAL, INTENT(IN)                         :: b(1)
INTEGER, INTENT(IN)                      :: n1
INTEGER, INTENT(IN)                      :: n2


DO  j=1,n2
  DO  i=1,n1
    k=n1*(j-1)+i
    c(k)=a(k)-b(k)
  END DO
END DO
RETURN
END SUBROUTINE matsub

SUBROUTINE matmul (c,a,b,n1,n2,n3)

!*** C(N1*N3) IS OBTAINED FROM MULTIPLYING A(N1*N2) BY B(N2*N3)



REAL, INTENT(OUT)                        :: c(1)
REAL, INTENT(IN)                         :: a(1)
REAL, INTENT(IN)                         :: b(1)
INTEGER, INTENT(IN)                      :: n1
INTEGER, INTENT(IN)                      :: n2
INTEGER, INTENT(IN)                      :: n3


DO i=1,n1
  DO k=1,n3
    n=n1*(k-1)+i
    c(n)=0.0
    DO j=1,n2
      l=n1*(j-1)+i
      m=n2*(k-1)+j
      c(n)=c(n)+a(l)*b(m)
    END DO
  END DO
END DO
RETURN
END SUBROUTINE matmul

SUBROUTINE matsca (d,v1,v2,n)

!*** D IS THE SCALAR PRODUCT OF VECTOR V1(N*1) AND V2(N*1)



REAL, INTENT(OUT)                        :: d
REAL, INTENT(IN)                         :: v1(1)
REAL, INTENT(IN)                         :: v2(1)
INTEGER, INTENT(IN)                      :: n


d=0.
DO  i=1,n
  d=d+v1(i)*v2(i)
END DO
RETURN
END SUBROUTINE matsca


!          SUBROUTINE MATINV

!          PURPOSE
!             COMPUTE THE INVERSE OF A NON-SINGLAR MATRIX

!          USAGE
!             CALL MATINV(AI,A,D,N)

!          DESCRIPTION OF PARAMETERS
!             A - NAME OF INPUT MATRIX
!             AI- NAME OF OUTPUT MATRIX (MAY BE THE SAME AS A)
!             D - DETERMINANT OF A
!             N - NUMBER OF ROWS AND COLUMNS OF A

!          REMARKS
!             ALL MATRICES ARE STORED AS GENERAL MATRICES

!          SUBROUTINES AND FUNCTION SUBPROGRAMS RQUIRED
!             SYSTEM ROUTINE -MATRIX-

!          METHOD
!        SET UP TO CALL GAUSS-JORDAN INVERSION ROUTINE.

!     ..................................................................

SUBROUTINE matinv(ai,a,d,n)


REAL, INTENT(OUT)                        :: ai(1)
REAL, INTENT(IN)                         :: a(1)
REAL, INTENT(OUT)                        :: d
INTEGER, INTENT(IN)                      :: n


!     SET UP TO CALL ROUTINE THAT ACTUALLY DOES THE CONVERSION

!DC      INTEGER ZZZ112(3)
!DC      DATA ZZZ112/L"MATRIX IS ",L"SINGULAR  ",L" "/
mx = n
m = n
epsil = 0.000 000 000 1

!         COPY A MATRIX INTO AI MATRIX

n2=n*n
DO  i = 1, n2
  ai(i)=a(i)
END DO

CALL matfs(epsil,mx,m,ai,ier,det)
d=det
IF(ier == 0) RETURN
d=0
PRINT *,' MATRIX IS SINGULAR '
x=-2
y=SQRT(x)**2
RETURN
END SUBROUTINE matinv

SUBROUTINE matfs(epsil,mx,m,a,ier,det)
!      MATFS IS A SINGLE-PRECISION MATRIX INVERSION SUBROUTINE
!     WRITTEN IN FORTRAN IV  - VARIABLE DIMENSIONING IS USED
!     OF MAX DIMENSIONS OF MATRIX IN CSLLING PROGRAM MUST BE SAME
!     VALUE AS PASSED ON BY CALLING SEQUENCE AS MX.
!     MATRIX CAN BE OF GENERAL FORM. METHOD IS GAUSS-JORDAN.

REAL, INTENT(IN OUT)                     :: epsil
INTEGER, INTENT(IN OUT)                  :: mx
INTEGER, INTENT(IN)                      :: m
REAL, INTENT(IN OUT)                     :: a(mx,mx)
INTEGER, INTENT(OUT)                     :: ier
REAL, INTENT(OUT)                        :: det
DIMENSION f(30),g(30),l(30),beta(30),temp(30)
INTEGER :: i,j,k,kp,f,beta,l,g,phi, fkp,lda,gkp
REAL :: mu
!     INITIALIZE AND LOGICAL OPERATIONS ON COLUMN K, PIVOT COL.
det=1.0
ier =0
DO  kp =1,m
  f(kp) = 0
  beta(kp) = 0
  l(kp)=kp
  g(kp)=kp
END DO
!     TRANSFORM BY COLUMNS, K
DO  k = 1, m
!     FIND LARGEST ELEMENT IN COL K WITH BETA STILL ZERO
  mu=0.0
  DO  i = 1,m
    IF (beta(i) < 0.0) THEN
      GO TO  1000
    ELSE IF (beta(i) == 0.0) THEN
      GO TO    10
    ELSE
      GO TO    14
    END IF
    10 IF ( ABS(a(i,k)) -  ABS(mu) > 0.0) THEN
      GO TO    12
    ELSE
      GO TO    14
    END IF
!     LABEL ROW WITH LARGEST ELEMENT (MU) AS PHI
    12 mu = a(i,k)
    phi = i
  END DO
!     IF ELEMENT MU IS TOO SMALL,MATRIX IS SINGULAR-CALL EXIT
  15 IF ( ABS(mu) - epsil > 0) THEN
    GO TO    18
  END IF
  16   ier =1
  RETURN
  18 det = mu*det
!     SETF(K) WITH INDEX OF ROW, BETA OF ROW NOW 1
  f(k) = phi
  beta(phi) = 1
!     ARITHMETIC OPERATIONS ON PIVOT COLUMN,K
!     FIRST SAVE ENTIRE COLUMN IN PRESENT FORM
  20 DO  i=1,m
    temp(i) = a(i,k)
  END DO
!     TRANSFORM COLUMN
  a(phi,k) = 1.0/mu
  DO  i=1,m
    IF (i - phi == 0) THEN
      GO TO    23
    END IF
    22 a(i,k) = -a(i,k)/mu
  END DO
!     LOGICAL OPERATIONS ON OTHER COLUMNS    PANEL IV
!     NOTE WE USE ORIG SAVED VALUES FROM PIVOT COLUMN,K
  DO  j =1,m
!     TEST TO SKIP COL K, ALREADY TRANSFORMED
    IF (j-k == 0) THEN
      GO TO    31
    END IF
!     NOT K   IF VALUE IN ROW PHI IS 0 SKIP COLUMN
    25 IF (a(phi,j) == 0.0) THEN
      GO TO    31
    END IF
!     NOT ZERO
    26 alpha = a(phi,j)
    a(phi,j) = alpha/mu
!     TRANSFORM IN ONE COL,J, BY ROW INDICES
    DO  i =1,m
!     TEST IF ELEMENT IS IN ROW WITH PIVOT ELEMENT
      IF (i-phi == 0) THEN
        GO TO    30
      END IF
!     NO
      28 a(i,j) = a(i,j) - alpha*temp(i) /mu
!     YES
    END DO
  END DO
END DO
!     PERMUTATIONS OF ROWS AND COLUMNS
mm=m-1
DO  kp=1,mm
!     PERMUTATION OF ROWS
  IF(g(kp)-f(kp) == 0.0) THEN
    GO TO    55
  END IF
  50 fkp = f(kp)
  lda = l(fkp)
  DO  j =1,m
    temp(j) = a(kp,j)
    a(kp,j) = a(lda,j)
    a(lda,j) = temp(j)
    det = -det
  END DO
!     PERMUTATION OF COLUMNS
  gkp = g(kp)
  DO  i = 1,m
    temp(i) = a(i,fkp)
    a(i,fkp) = a(i,gkp)
    a(i,gkp) = temp(i)
!     RESET INDICES
    l(fkp) = k
    l(gkp) = lda
    g(lda) = gkp
    g(kp) = fkp
  END DO
END DO
RETURN
1000  WRITE (6,1001)
STOP
1001 FORMAT (22H error, beta negative )
!     INVERTED MATRIX IS IN CELLS ORIGINALLY OCCUPIED BY MATRIX
!     DETERMINANT IS IN CELL DET (NOT RETURNED)
60 RETURN
END SUBROUTINE matfs

SUBROUTINE matcon (c,con,a,n1,n2)

!*** C(N1*N2) IS OBTAINED FROM MULTIPLYING COSTANT CON BY A(N1*N2)



REAL, INTENT(OUT)                        :: c(1)
REAL, INTENT(IN)                         :: con
REAL, INTENT(IN)                         :: a(1)
INTEGER, INTENT(IN)                      :: n1
INTEGER, INTENT(IN)                      :: n2


DO  j=1,n2
  DO  i=1,n1
    k=n2*(j-1)+i
    c(k)=a(k)*con
  END DO
END DO
RETURN
END SUBROUTINE matcon

SUBROUTINE mattra (c,a,n1,n2)

!*** C(N2*N1) IS THE TRANSPOSE OF A(N1*N2)



REAL, INTENT(OUT)                        :: c(n2,n1)
REAL, INTENT(IN)                         :: a(n1,n2)
INTEGER, INTENT(IN)                      :: n1
INTEGER, INTENT(IN)                      :: n2


DO  j=1,n2
  DO  i=1,n1
    c(j,i)=a(i,j)
  END DO
END DO
RETURN
END SUBROUTINE mattra

SUBROUTINE mattrf(c,t,a,n)

!*** C(N*N) IS OBTAINED FROM A(N*N) BY THE ORTHOGONAL TRANSFORMATION
!    MATRIX T(N*N)    C = T * A * T(TRANSPOSE)



REAL, INTENT(OUT)                        :: c(n,n)
REAL, INTENT(IN)                         :: t(n,n)
REAL, INTENT(IN)                         :: a(n,n)
INTEGER, INTENT(IN)                      :: n


DO  i=1,n
  DO  j=1,n
    c(i,j)=0.
    DO  k=1,n
      DO  l=1,n
        c(i,j)=c(i,j)+t(i,k)*a(k,l)*t(j,l)
      END DO
    END DO
  END DO
END DO

RETURN
END SUBROUTINE mattrf

SUBROUTINE matsks (c,v)

!*** SKEW-SYMMETRIC MATRIX C(3*3) IS OBTAINED FROM VECTOR C(3*1)



REAL, INTENT(OUT)                        :: c(3,3)
REAL, INTENT(IN)                         :: v(3)


c(1,1)=0.
c(1,2)=-v(3)
c(1,3)=v(2)
c(2,1)=v(3)
c(2,2)=0.
c(2,3)=-v(1)
c(3,1)=-v(2)
c(3,2)=v(1)
c(3,3)=0.

RETURN
END SUBROUTINE matsks

SUBROUTINE matvsk (v,a)

!*** VECTOR V(3*1) IS OBTAINED FROM SKEW-SYMMETRIC MATRIX A(3*3)



REAL, INTENT(OUT)                        :: v(3)
REAL, INTENT(IN)                         :: a(3,3)


v(1)=-a(2,3)
v(2)=a(1,3)
v(3)=-a(1,2)

RETURN
END SUBROUTINE matvsk

SUBROUTINE matzer (a,n1,n2)

!*** CREATION OF A ZERO MATRIX A(N1*N2)



REAL, INTENT(OUT)                        :: a(1)
INTEGER, INTENT(IN)                      :: n1
INTEGER, INTENT(IN)                      :: n2


l=n1*n2
DO  i=1,l
  a(i)=0.
END DO
RETURN
END SUBROUTINE matzer

SUBROUTINE matuni (a,n)

!*** CREATION OF A UNIT MATRIX A(N*N)



REAL, INTENT(OUT)                        :: a(1)
INTEGER, INTENT(IN)                      :: n


DO  j=1,n
  DO  i=1,n
    k=n*(j-1) + i
    a(k)=0.
  END DO
  k=n*(j-1) + j
  a(k)=1.
END DO
RETURN
END SUBROUTINE matuni

SUBROUTINE matdia (a,d,n)

!*** A(N*N) IS THE DIAGONAL MATRIX OBTAINED FROM VECTOR D(N*1)



REAL, INTENT(OUT)                        :: a(n,n)
REAL, INTENT(IN)                         :: d(n)
INTEGER, INTENT(IN)                      :: n


DO  j=1,n
  DO  i=1,n
    a(i,j)=0.
  END DO
  a(j,j)=d(j)
END DO
RETURN
END SUBROUTINE matdia


SUBROUTINE matvdi (d,a,n)

!*** VECTOR D(N*1) IS THE DIAGONAL OF MATRIX A(N*N)



REAL, INTENT(OUT)                        :: d(n)
REAL, INTENT(IN)                         :: a(n,n)
INTEGER, INTENT(IN)                      :: n


DO  i=1,n
  d(i)=a(i,i)
END DO

RETURN
END SUBROUTINE matvdi

SUBROUTINE mateql (c,a,n1,n2)

!*** C(N1*N2) IS SET EQUAL TO A(N1*N2)



REAL, INTENT(OUT)                        :: c(1)
REAL, INTENT(IN)                         :: a(1)
INTEGER, INTENT(IN)                      :: n1
INTEGER, INTENT(IN)                      :: n2


l=n1*n2
DO  i=1,l
  c(i)=a(i)
END DO
RETURN
END SUBROUTINE mateql

SUBROUTINE matabs (d,v,n)

!*** D IS THE ABSOLUTE VALUE OF VECTOR V(N*1)



REAL, INTENT(OUT)                        :: d
REAL, INTENT(IN)                         :: v(1)
INTEGER, INTENT(IN)                      :: n


dv=0.
DO  i=1,n
  dv=dv+v(i)*v(i)
END DO
d=SQRT(dv)
RETURN
END SUBROUTINE matabs

SUBROUTINE matcar(sbel,dbe,azbel,elbel)

!*** CARTESIAN COORDINATES SBEL(3X1) FROM POLAR COORDINATES



REAL, INTENT(OUT)                        :: sbel(3)
REAL, INTENT(IN)                         :: dbe
REAL, INTENT(IN OUT)                     :: azbel
REAL, INTENT(IN OUT)                     :: elbel


celb=COS(elbel)
selb=SIN(elbel)
cazb=COS(azbel)
sazb=SIN(azbel)

sbel(1)=dbe*celb*cazb
sbel(2)=dbe*celb*sazb
sbel(3)=-dbe*selb

RETURN
END SUBROUTINE matcar

SUBROUTINE matpol(dbe,azbel,elbel,sbel)

!*** POLAR COORDINATES FROM CARTESIAN COORDINATES



REAL, INTENT(OUT)                        :: dbe
REAL, INTENT(OUT)                        :: azbel
REAL, INTENT(OUT)                        :: elbel
REAL, INTENT(IN)                         :: sbel(3)


IF(AMAX1(ABS(sbel(1)),ABS(sbel(2))) < 1E-10) THEN
  azbel=0.
  elbel=0.
  dum1=0.
ELSE
  azbel=ATAN2(sbel(2),sbel(1))
  dum1=sbel(1)*sbel(1)+sbel(2)*sbel(2)
  dum2=SQRT(dum1)
  elbel=ATAN2(-sbel(3),dum2)
END IF
dum3=dum1+sbel(3)*sbel(3)
dbe=SQRT(dum3)

RETURN
END SUBROUTINE matpol

SUBROUTINE mat2tr(t,psi,tht)

!*** T IS THE TRANSFORMATION MATRIX OBTAINED FROM TWO
!*** ANGLE TRANSFORMATIONS PSI AND THT



REAL, INTENT(OUT)                        :: t(3,3)
REAL, INTENT(IN OUT)                     :: psi
REAL, INTENT(IN OUT)                     :: tht


t(1,3)=-SIN(tht)
t(2,1)=-SIN(psi)
t(2,2)=COS(psi)
t(3,3)=COS(tht)
t(1,1)=t(3,3)*t(2,2)
t(1,2)=-t(3,3)*t(2,1)
t(3,1)=-t(1,3)*t(2,2)
t(3,2)=t(1,3)*t(2,1)
t(2,3)=0.

RETURN
END SUBROUTINE mat2tr

SUBROUTINE mat3tr(t,psi,tht,phi)

!*** T IS THE TRANSFORMATION MATRIX OBTAINED FROM THREE
!*** ANGLE TRANSFORMATIONS PSI, THT AND PHI (IN THIS SEQUENCE)
!*** EULER ANGLES OF FLIGHT MECHANICS



REAL, INTENT(OUT)                        :: t(3,3)
REAL, INTENT(IN OUT)                     :: psi
REAL, INTENT(IN OUT)                     :: tht
REAL, INTENT(IN OUT)                     :: phi


cpsi=COS(psi)
spsi=SIN(psi)
ctht=COS(tht)
stht=SIN(tht)
cphi=COS(phi)
sphi=SIN(phi)
t(1,1)=cpsi*ctht
t(1,2)=spsi*ctht
t(1,3)=-stht
t(2,1)=-spsi*cphi+cpsi*stht*sphi
t(2,2)=cpsi*cphi+spsi*stht*sphi
t(2,3)=ctht*sphi
t(3,1)=spsi*sphi+cpsi*stht*cphi
t(3,2)=-cpsi*sphi+spsi*stht*cphi
t(3,3)=ctht*cphi

RETURN
END SUBROUTINE mat3tr

SUBROUTINE mat3eu(t,psi,alam,ome)

!*** TRANSF. MATRIX OF THE THREE EULER ANGLES PSI,ALAM,OME (IN THIS SEQ.)
!*** EULER ANGLES OF GYRODYNAMICS, NOT FLIGHT MECHANICS.



REAL, INTENT(OUT)                        :: t(3,3)
REAL, INTENT(IN OUT)                     :: psi
REAL, INTENT(IN OUT)                     :: alam
REAL, INTENT(IN OUT)                     :: ome


come=COS(ome)
some=SIN(ome)
calam=COS(alam)
salam=SIN(alam)
cpsi=COS(psi)
spsi=SIN(psi)

sc=some*calam
cc=come*calam

t(1,1)=come*cpsi-sc*spsi
t(1,2)=come*spsi+sc*cpsi
t(1,3)=some*salam
t(2,1)=-some*cpsi-cc*spsi
t(2,2)=-some*spsi+cc*cpsi
t(2,3)=come*salam
t(3,1)=salam*spsi
t(3,2)=-salam*cpsi
t(3,3)=calam

RETURN
END SUBROUTINE mat3eu

SUBROUTINE matrot(r,a,b)

!*** ROTATION TENSOR R(3X3) FROM ANGLE OF ROTATION A AND
!    VECTOR OF ROTATION B(3X1)



REAL, INTENT(OUT)                        :: r(3,3)
REAL, INTENT(IN OUT)                     :: a
REAL, INTENT(IN)                         :: b(3)


ca=COS(a)
sa=SIN(a)
oca=1.-ca
b12oca=b(1)*b(2)*oca
b13oca=b(1)*b(2)*oca
b23oca=b(2)*b(3)*oca

r(1,1)=ca+b(1)*b(1)*oca
r(1,2)=b12oca-b(3)*sa
r(1,3)=b13oca+b(2)*sa
r(2,1)=b12oca+b(3)*sa
r(2,2)=ca+b(2)*b(2)*oca
r(2,3)=b23oca-b(1)*sa
r(3,1)=b13oca-b(2)*sa
r(3,2)=b23oca+b(1)*sa
r(3,3)=ca+b(3)*b(3)*oca

RETURN
END SUBROUTINE matrot

!*** CHOLESKY DECOMPOSITION. LOWER TRIANGULAR MATRIX A(N,N) WHICH IS THE
!    SQUARE ROOT OF MATRIX P(N,N)

SUBROUTINE matcho(a,p,n)



REAL, INTENT(IN OUT)                     :: a(n,n)
REAL, INTENT(IN OUT)                     :: p(n,n)
INTEGER, INTENT(IN)                      :: n


DO  i=1,n
  DO  j=1,n
    
    IF(j < i)THEN
      sum=0.
      IF(j > 1)THEN
        loop20:  DO  k=1,j-1
          20 sum=sum+a(i,k)*a(j,k)
        END IF
        IF(a(j,j) == 0.)THEN
          a(i,j)=0.
        ELSE
          a(i,j)=(p(i,j)-sum)/a(j,j)
        END IF
        
      ELSE IF(j == i)THEN
        sum=0.
        IF(i > 1)THEN
          DO  k=1,i-1
            30 sum=sum+a(i,k)**2
          END IF
          a(i,j)=SQRT(p(i,i)-sum)
          
        ELSE
          a(i,j)=0.
          
        END IF
        
        10 CONTINUE
        
        RETURN
      END SUBROUTINE matcho
      
!***********************************************************************
!****************** VECTOR UTILITIES ***********************************
!***********************************************************************
      
      SUBROUTINE vecucr(z,x,y)
      
!*** PERFORMS UNIT CROSS PRODUCT OF 3-VECTORS
!     NAMELY: Z=UNIT(X cross Y)
      
      
      
      REAL, INTENT(OUT)                        :: z(3)
      REAL, INTENT(IN)                         :: x(3)
      REAL, INTENT(IN)                         :: y(3)
      
      z(1) = x(2)*y(3)-x(3)*y(2)
      z(2) = x(3)*y(1)-x(1)*y(3)
      z(3) = x(1)*y(2)-x(2)*y(1)
      zm=fnrss(z,3)
      z(1)=z(1)/zm
      z(2)=z(2)/zm
      z(3)=z(3)/zm
      RETURN
    END SUBROUTINE vecucr
    
    SUBROUTINE vecvec(d,a,b,c)
    
!*** CONSTRUCT VECTOR D FROM ELEMENTS A,B,C
    
    
    
    REAL, INTENT(OUT)                        :: d(3)
    REAL, INTENT(IN)                         :: a
    REAL, INTENT(IN)                         :: b
    REAL, INTENT(IN)                         :: c
    
    d(1)=a
    d(2)=b
    d(3)=c
    RETURN
  END SUBROUTINE vecvec
  
  SUBROUTINE vecuvc(d,a,b,c)
  
!*** CONSTRUCT UNIT VECTOR FROM ELEMENTS A,B,C
  
  
  
  REAL, INTENT(OUT)                        :: d(3)
  REAL, INTENT(IN)                         :: a
  REAL, INTENT(IN)                         :: b
  REAL, INTENT(IN)                         :: c
  
  CALL vecvec(d,a,b,c)
  rssd=fnrss(d,3)
  d(1)=a/rssd
  d(2)=b/rssd
  d(3)=c/rssd
  RETURN
END SUBROUTINE vecuvc

SUBROUTINE vecang(a,b,c)

!*** ANGLE A(RAD) BETWEEN VECTOR B(3X1) AND C(3X1)



REAL, INTENT(OUT)                        :: a
REAL, INTENT(IN)                         :: b(3)
REAL, INTENT(IN)                         :: c(3)

sca=b(1)*c(1)+b(2)*c(2)+b(3)*c(3)
bsq=b(1)*b(1)+b(2)*b(2)+b(3)*b(3)
csq=c(1)*c(1)+c(2)*c(2)+c(3)*c(3)
dumm=bsq*csq
!ZI970430      IF(DUMM.NE.0.) THEN
IF(dumm > 1.e-5) THEN
  dum=sca/SQRT(dumm)
ELSE
  dum=1.
END IF
IF(ABS(dum) > 1.) dum=SIGN(1.,dum)
a=ACOS(dum)
RETURN
END SUBROUTINE vecang

FUNCTION fnrss(r,n)

!*** FUNCTION CONSTRUCTS ROOT-SUM-SQUARE OF N-VECTOR R



REAL, INTENT(IN)                         :: r(n)
INTEGER, INTENT(IN)                      :: n

sum=0.
DO  i=1,n
  sum=sum+r(i)**2
END DO
fnrss=SQRT(sum)
RETURN
END FUNCTION fnrss

!******************************************************************

FUNCTION fngaus(xmean,sigma)

!*** GAUSSIAN DISTRIBUTION

v1=ranf()
v2=ranf()
fngaus=sigma*SQRT(2.*LOG(1./v1))*COS(6.2831853072*v2)+xmean
END FUNCTION fngaus

!**********************************************************************
!*** Random Number Function Generator                               ***

REAL*4 FUNCTION ranf()
CALL random_number( rvalue )
DO WHILE( rvalue == 0.0 )
  CALL random_number( rvalue )
END DO
ranf = rvalue
RETURN
END FUNCTION ranf
!***********************************************************************
!*************** TABLE LOOK-UP UTILITIES *******************************
!***********************************************************************

SUBROUTINE table(x,xi,yi,nx,y)



REAL, INTENT(OUT)                        :: x
REAL, INTENT(IN)                         :: xi(1)
REAL, INTENT(IN)                         :: yi(1)
INTEGER, INTENT(IN)                      :: nx
REAL, INTENT(OUT)                        :: y

IF(x < xi(1))x=xi(1)
IF(x > xi(nx))x=xi(nx)
DO  i = 2, nx
  IF(x <= xi(i)) GO TO 20
END DO
END DO
END DO
i = nx
pct = (x - xi(i-1)) / (xi(i) - xi(i-1))
END DO loop20
y = yi(i-1) + pct * (yi(i) - yi(i-1))
RETURN
END SUBROUTINE table

SUBROUTINE tabl2 (x,y,xy,zi,nxy,z)


REAL, INTENT(OUT)                        :: x
REAL, INTENT(OUT)                        :: y
REAL, INTENT(IN)                         :: xy(1)
REAL, INTENT(IN)                         :: zi(1)
INTEGER, INTENT(IN)                      :: nxy(2)
REAL, INTENT(OUT)                        :: z
REAL :: t(2)


nx = nxy(1)
ny = nxy(2)
IF(x < xy(1))x=xy(1)
IF(x > xy(nx))x=xy(nx)
IF(y < xy(nx+1))y=xy(nx+1)
IF(y > xy(nx+ny))y=xy(nx+ny)
DO  m = 2,ny
  j = nx + m
  IF (y <= xy(j)) GO TO 20
END DO
m = ny
20      pcty = (y - xy(j-1)) / (xy(j) - xy(j-1))
DO  i = 2,nx
  IF(x <= xy(i)) GO TO 40
END DO
END DO
i = nx
40      pctx = (x - xy(i-1)) / (xy(i) - xy(i-1))
DO  k = 1,2
  ky = nx * (m - k) + i
  t(k) = zi(ky-1) + pctx * (zi(ky) - zi(ky-1))
END DO
z = t(2) + pcty * (t(1) - t(2))
RETURN
END SUBROUTINE tabl2

SUBROUTINE tabl3 (x,y,z,xyz,zw,nxyz,w)


REAL, INTENT(OUT)                        :: x
REAL, INTENT(OUT)                        :: y
REAL, INTENT(OUT)                        :: z
REAL, INTENT(IN)                         :: xyz(1)
REAL, INTENT(IN)                         :: zw(1)
INTEGER, INTENT(IN)                      :: nxyz(3)
REAL, INTENT(OUT)                        :: w
REAL :: t(2,2), s(2)


nx = nxyz(1)
ny = nxyz(2)
nz = nxyz(3)
IF(x < xyz(1))x=xyz(1)
IF(x > xyz(nx))x=xyz(nx)
IF(y < xyz(nx+1))y=xyz(nx+1)
IF(y > xyz(nx+ny))y=xyz(nx+ny)
IF(z < xyz(nx+ny+1))z=xyz(nx+ny+1)
IF(z > xyz(nx+ny+nz))z=xyz(nx+ny+nz)
DO  i = 2,nx
  IF(x <= xyz(i)) GO TO 20
END DO
i = nx
20      pctx = (x - xyz(i-1)) / (xyz(i) - xyz(i-1))
DO  j = 2,ny
  l = nx + j
  IF(y <= xyz(l)) GO TO 40
END DO
j = ny
40      pcty = (y - xyz(l-1)) / (xyz(l) - xyz(l-1))
nxy = nx + ny
DO  k = 2,nz
  m = nxy + k
  IF(z <= xyz(m)) GO TO 60
END DO
k = nz
60    IF((xyz(m)-xyz(m-1)) > 1.e-10)GO TO 65
pctz=0.0
GO TO 67
65      pctz = (z - xyz(m-1)) / (xyz(m) - xyz(m-1))
67    nxny = nx * ny
DO  jj = 1,2
  jz = nxny * (k - jj)
  DO  kk = 1,2
    ky = jz + nx * (j - kk) + i
    t(kk,jj) = zw(ky-1) + pctx * (zw(ky) - zw(ky-1))
  END DO
END DO
DO  n = 1,2
  s(n) = t(2,n) + pcty * (t(1,n) - t(2,n))
END DO
w = s(2) + pctz * (s(1) - s(2))
RETURN
END SUBROUTINE tabl3

SUBROUTINE tabl4(x,y,z,w,xyzw,ww,nxyzw,ans)


REAL, INTENT(OUT)                        :: x
REAL, INTENT(OUT)                        :: y
REAL, INTENT(OUT)                        :: z
REAL, INTENT(OUT)                        :: w
REAL, INTENT(IN)                         :: xyzw(1)
REAL, INTENT(IN)                         :: ww(1)
INTEGER, INTENT(IN)                      :: nxyzw(4)
REAL, INTENT(OUT)                        :: ans
REAL :: t(2,2,2), r(2,2), s(2)


nx = nxyzw(1)
ny = nxyzw(2)
nz = nxyzw(3)
nw = nxyzw(4)
IF(x < xyzw(1))x=xyzw(1)
IF(x > xyzw(nx))x=xyzw(nx)
IF(y < xyzw(nx+1))y=xyzw(nx+1)
IF(y > xyzw(nx+ny))y=xyzw(nx+ny)
IF(z < xyzw(nx+ny+1))z=xyzw(nx+ny+1)
IF(z > xyzw(nx+ny+nz))z=xyzw(nx+ny+nz)
IF(w < xyzw(nx+ny+nz+1))w=xyzw(nx+ny+nz+1)
IF(w > xyzw(nx+ny+nz+nw))w=xyzw(nx+ny+nz+nw)
DO  i = 2,nx
  IF( x <= xyzw(i)) GO TO 20
END DO
i = nx
20      pctx = (x - xyzw(i-1)) / (xyzw(i) - xyzw(i-1))
DO  j = 2,ny
  l = nx + j
  IF( y <= xyzw(l)) GO TO 40
END DO
j = ny
40      pcty = (y - xyzw(l-1)) / (xyzw(l) - xyzw(l-1))
nxy = nx + ny
DO  k = 2,nz
  m = nxy + k
  IF(z <= xyzw(m)) GO TO 60
END DO
k = nz
60      pctz = (z - xyzw(m-1)) / (xyzw(m) - xyzw(m-1))
nxyz = nxy + nz
DO  l = 2,nw
  n = nxyz + l
  IF(w <= xyzw(n)) GO TO 80
END DO
l = nw
80      pctw = (w - xyzw(n-1)) / (xyzw(n) - xyzw(n-1))
nxny = nx * ny
nxnz = nxny * nz
DO  ll = 1,2
  lll = nxnz * (l-ll)
  DO  kk = 1,2
    kkk = lll + nxny * (k-kk)
    DO  jj=1,2
      jjj = kkk + nx * (j-jj) + i
      t(jj,kk,ll)=ww(jjj-1)+pctx*(ww(jjj)-ww(jjj-1))
    END DO
  END DO
END DO
DO  LT = 1,2
  DO  kt = 1,2
    r(kt,LT) = t(2,kt,LT) + pcty * (t(1,kt,LT) - t(2,kt,LT))
  END DO
END DO
DO  n = 1,2
  s(n) = r(2,n) + pctz * (r(1,n) - r(2,n))
END DO
ans = s(2) + pctw * (s(1) - s(2))
RETURN
END SUBROUTINE tabl4

!***********************************************************************
!*************** TABLE LOOK-UP WITH POINTERS ***************************
!***********************************************************************

!#######################################################################

SUBROUTINE tblp1(x,ix,xt,mx,yt,y,yd)

!     VERSION 1.1   CONFIGURATION # 5.20
!#######################################################################

!     TBLP1 IS A GENERAL ONE-DIMENSIONAL TABLE LOOK-UP WITH POINTERS TO
!     ENHANCE SPEED


!     INPUTS:

!       X  - INDEPENDENT VARIABLE
!       IX - POINTER TO LAST PLACE IN TABLE WHERE LOOKUP OCCURED FOR X
!       XT - ARRAY OF INDEPENDENT VARIABLE VALUES (MX VALUES)
!       MX - MAXIMUN OF INDEPENDENT VARIABLE VALUES IN TABLE
!       YT - MATRIX OF DEPENDENT VARIABLES (MX VALUES)


!     OUTPUTS:

!       Y  - DEPENDENT VALUE FROM TABLE LOOK UP
!       YD - SLOPE OF Y WITH RESPECT TO THE INDEPENDENT VARIABLE

!#######################################################################


REAL, INTENT(IN)                         :: x
INTEGER, INTENT(OUT)                     :: ix
REAL, INTENT(IN)                         :: xt(mx)
INTEGER, INTENT(IN)                      :: mx
REAL, INTENT(IN)                         :: yt(mx)
REAL, INTENT(OUT)                        :: y
REAL, INTENT(OUT)                        :: yd

LOGICAL :: ascnd

!--FIND POINTER FOR INDEPENDENT VARIABLE

ascnd = xt(mx) >= xt(1)
ix = MAX0(1,MIN0(ix,mx))
IF (x > xt(ix).EQV.ascnd) THEN
  
!--SEARCH UP FOR INDEPENDENT VARIABLE
  
  DO   i=ix+1,mx
    IF (xt(i) > x.EQV.ascnd) THEN
      ix = i-1
      GO TO 30
    END IF
  END DO
  ix = mx-1
  
!--SEARCH DOWN FOR INDEPENDENT VARIABLE
  
ELSE
  DO   i=ix-1,1,-1
    IF (xt(i) <= x.EQV.ascnd) THEN
      ix = i
      GO TO 30
    END IF
  END DO
  ix = 1
END IF

!--NOW HAVE UPDATED POINTER -- DEFINE SOME VARIABLES FOR LATER USE

30 dtx = xt(ix)-xt(ix+1)
dx  = x-xt(ix)
!99      IF (DTX.EQ.0.D0) THEN
!99      CALL ERROR(2010)
!99      ELSE

!--COMPUTE DERIVATIVE OF DATA WITH RESPECT TO INDEPENDENT VARIABLE

yd = (yt(ix)-yt(ix+1))/dtx

!--INTERPOLATE

y = yt(ix) + yd*dx
!99      END IF
RETURN
END SUBROUTINE tblp1
!#######################################################################

SUBROUTINE tblp2(x1,x2,ix1,ix2,xt1,xt2,mx1,mx2,yt,y,yd1,yd2)

!     VERSION 1.1   CONFIGURATION # 5.21
!#######################################################################

!     TBLP2 IS A GENERAL TWO-DIMENSIONAL TABLE LOOK-UP WITH POINTERS TO
!     ENHANCE SPEED

!     INPUTS:

!       X1  - INDEPENDENT VARIABLE 1
!       X2  - INDEPENDENT VARIABLE 2
!       IX1 - POINTER TO LAST TABLE LOOKUP POSITION FOR X1
!       IX2 - POINTER TO LAST TABLE LOOKUP POSITION FOR X2
!       XT1 - ARRAY OF INDEPENDENT VARIABLE 1 VALUES (MX1 VALUES)
!       XT2 - ARRAY OF INDEPENDENT VARIABLE 2 VALUES (MX2 VALUES)
!       YT  - MATRIX OF DEPENDENT VARIABLES (MX1*MX2 VALUES)
!       MX1 - MAXIMUN OF INDEPENDENT VARIABLE 1 VALUES IN TABLE
!       MX2 - MAXIMUN OF INDEPENDENT VARIABLE 2 VALUES IN TABLE


!     OUTPUTS:

!       Y   - DEPENDENT VALUE FROM TABLE LOOK UP
!       YD1 - SLOPE OF Y WITH RESPECT TO INDEPENDENT VARIABLE 1
!       YD2 - SLOPE OF Y WITH RESPECT TO INDEPENDENT VARIABLE 2

!#######################################################################


REAL, INTENT(IN)                         :: x1
REAL, INTENT(IN)                         :: x2
INTEGER, INTENT(OUT)                     :: ix1
INTEGER, INTENT(OUT)                     :: ix2
REAL, INTENT(IN)                         :: xt1(mx1)
REAL, INTENT(IN)                         :: xt2(mx2)
INTEGER, INTENT(IN)                      :: mx1
INTEGER, INTENT(IN)                      :: mx2
REAL, INTENT(IN)                         :: yt(mx1,mx2)
REAL, INTENT(OUT)                        :: y
REAL, INTENT(OUT)                        :: yd1
REAL, INTENT(OUT)                        :: yd2

LOGICAL :: ascnd

!--FIND POINTER FOR INDEPENDENT VARIABLE 1

ascnd = xt1(mx1) >= xt1(1)
ix1 = MAX0(1,MIN0(ix1,mx1))
IF (x1 > xt1(ix1).EQV.ascnd) THEN
  
!--SEARCH UP FOR INDEPENDENT VARIABLE 1
  
  DO   i=ix1+1,mx1
    IF (xt1(i) > x1.EQV.ascnd) THEN
      ix1 = i-1
      GO TO 30
    END IF
  END DO
  ix1 = mx1-1
  
!--SEARCH DOWN FOR INDEPENDENT VARIABLE 1
  
ELSE
  DO   i=ix1-1,1,-1
    IF (xt1(i) <= x1.EQV.ascnd) THEN
      ix1 = i
      GO TO 30
    END IF
  END DO
  ix1 = 1
END IF

!--FIND POINTER FOR INDEPENDENT VARIABLE 2

30 ascnd = xt2(mx2) >= xt2(1)
ix2 = MAX0(1,MIN0(ix2,mx2))
IF (x2 > xt2(ix2).EQV.ascnd) THEN
  
!--SEARCH UP FOR INDEPENDENT VARIABLE 2
  
  DO   i=ix2+1,mx2
    IF (xt2(i) > x2.EQV.ascnd) THEN
      ix2 = i-1
      GO TO 60
    END IF
  END DO
  ix2 = mx2-1
  
!--SEARCH DOWN FOR INDEPENDENT VARIABLE 2
  
ELSE
  DO   i=ix2-1,1,-1
    IF (xt2(i) <= x2.EQV.ascnd) THEN
      ix2 = i
      GO TO 60
    END IF
  END DO
  ix2 = 1
END IF

!--NOW HAVE UPDATED IX1 AND IX2 -- DEFINE SOME VARIABLES FOR LATER USE

60 dtx1 = xt1(ix1)-xt1(ix1+1)
dtx2 = xt2(ix2)-xt2(ix2+1)
!99      IF (DTX1.EQ.0.D0.OR.DTX2.EQ.0.D0) THEN
!99 CALL ERROR(2011)
!99      ELSE
IF (dtx1 == 0..OR.dtx2 == 0.) GO TO 70
dx1 = x1-xt1(ix1)
dx2 = x2-xt2(ix2)
s1  = (yt(ix1,ix2)-yt(ix1+1,ix2))/dtx1
s3  = (yt(ix1,ix2+1)-yt(ix1+1,ix2+1))/dtx1
y1  = yt(ix1,ix2)+s1*dx1
y3  = yt(ix1,ix2+1)+s3*dx1

!--COMPUTE DERIVATIVES OF DATA WITH RESPECT TO INDEPENDENT VARIABLES

yd2 = (y1-y3)/dtx2

!--ERROR FIY MG 2-21-90

!        YD1 = (S3-S1)/DTX2*DX2+S1
yd1 = (s1-s3)/dtx2*dx2+s1

!--INTERPOLATE

y = y1+yd2*dx2
!99      END IF
70 CONTINUE
RETURN
END SUBROUTINE tblp2
!#######################################################################

SUBROUTINE tblp3(x1,x2,x3,ix1,ix2,ix3,xt1,xt2,xt3,mx1,mx2,mx3,  &
    yt,y,yd1,yd2,yd3)

!     VERSION 1.1   CONFIGURATION # 5.22
!#######################################################################

!     TBLP3 IS A GENERAL THREE-DIMENSIONAL TABLE LOOK-UP WITH POINTERS
!     TO ENHANCE SPEED


!     INPUTS:

!       X1  - INDEPENDENT VARIABLE 1
!       X2  - INDEPENDENT VARIABLE 2
!       X3  - INDEPENDENT VARIABLE 3
!       IX1 - POINTER TO LAST TABLE LOOKUP POSITION FOR X1
!       IX2 - POINTER TO LAST TABLE LOOKUP POSITION FOR X2
!       IX3 - POINTER TO LAST TABLE LOOKUP POSITION FOR X3
!       XT1 - ARRAY OF INDEPENDENT VARIABLE 1 VALUES (MX1 VALUES)
!       XT2 - ARRAY OF INDEPENDENT VARIABLE 2 VALUES (MX2 VALUES)
!       XT3 - ARRAY OF INDEPENDENT VARIABLE 3 VALUES (MX3 VALUES)
!       YT  - MATRIX OF DEPENDENT VARIABLES (MX1*MX2*MX3 VALUES)
!       MX1 - MAXIMUM OF INDEPENDENT VARIABLE 1 VALUES IN TABLE
!       MX2 - MAXIMUM OF INDEPENDENT VARIABLE 2 VALUES IN TABLE
!       MX3 - MAXIMUM OF INDEPENDENT VARIABLE 3 VALUES IN TABLE


!     OUTPUTS:

!       Y   - DEPENDENT VALUE FROM TABLE LOOK UP
!       YD1 - SLOPE OF Y WITH RESPECT TO INDEPENDENT VARIABLE 1
!       YD2 - SLOPE OF Y WITH RESPECT TO INDEPENDENT VARIABLE 2
!       YD3 - SLOPE OF Y WITH RESPECT TO INDEPENDENT VARIABLE 3

!#######################################################################


REAL, INTENT(IN)                         :: x1
REAL, INTENT(IN)                         :: x2
REAL, INTENT(IN)                         :: x3
INTEGER, INTENT(OUT)                     :: ix1
INTEGER, INTENT(OUT)                     :: ix2
INTEGER, INTENT(OUT)                     :: ix3
REAL, INTENT(IN)                         :: xt1(mx1)
REAL, INTENT(IN)                         :: xt2(mx2)
REAL, INTENT(IN)                         :: xt3(mx3)
INTEGER, INTENT(IN)                      :: mx1
INTEGER, INTENT(IN)                      :: mx2
INTEGER, INTENT(IN)                      :: mx3
REAL, INTENT(IN)                         :: yt(mx1,mx2,mx3)
REAL, INTENT(OUT)                        :: y
REAL, INTENT(OUT)                        :: yd1
REAL, INTENT(OUT)                        :: yd2
REAL, INTENT(OUT)                        :: yd3

LOGICAL :: ascnd

!--FIND POINTER FOR INDEPENDENT VARIABLE 1

ascnd = xt1(mx1) >= xt1(1)
ix1 = MAX0(1,MIN0(ix1,mx1))
IF (x1 > xt1(ix1).EQV.ascnd) THEN
  
!--SEARCH UP FOR INDEPENDENT VARIABLE 1
  
  DO   i=ix1+1,mx1
    IF (xt1(i) > x1.EQV.ascnd) THEN
      ix1 = i-1
      GO TO 30
    END IF
  END DO
  ix1 = mx1-1
  
!--SEARCH DOWN FOR INDEPENDENT VARIABLE 1
  
ELSE
  DO   i=ix1-1,1,-1
    IF (xt1(i) <= x1.EQV.ascnd) THEN
      ix1 = i
      GO TO 30
    END IF
  END DO
  ix1 = 1
END IF

!--FIND POINTER FOR INDEPENDENT VARIABLE 2

30 ascnd = xt2(mx2) >= xt2(1)
ix2 = MAX0(1,MIN0(ix2,mx2))
IF (x2 > xt2(ix2).EQV.ascnd) THEN
  
!--SEARCH UP FOR INDEPENDENT VARIABLE 2
  
  DO   i=ix2+1,mx2
    IF (xt2(i) > x2.EQV.ascnd) THEN
      ix2 = i-1
      GO TO 60
    END IF
  END DO
  ix2 = mx2-1
  
!--SEARCH DOWN FOR INDEPENDENT VARIABLE 2
  
ELSE
  DO   i=ix2-1,1,-1
    IF (xt2(i) <= x2.EQV.ascnd) THEN
      ix2 = i
      GO TO 60
    END IF
  END DO
  ix2 = 1
END IF

!--FIND POINTER FOR INDEPENDENT VARIABLE 3

60 ascnd = xt3(mx3) >= xt3(1)
ix3 = MAX0(1,MIN0(ix3,mx3))
IF (x3 > xt3(ix3).EQV.ascnd) THEN
  
!--SEARCH UP FOR INDEPENDENT VARIABLE 3
  
  DO   i=ix3+1,mx3
    IF (xt3(i) > x3.EQV.ascnd) THEN
      ix3 = i-1
      GO TO 90
    END IF
  END DO
  ix3 = mx3-1
  
!--SEARCH DOWN FOR INDEPENDENT VARIABLE 3
  
ELSE
  DO   i=ix3-1,1,-1
    IF (xt3(i) <= x3.EQV.ascnd) THEN
      ix3 = i
      GO TO 90
    END IF
  END DO
  ix3 = 1
END IF

!--NOW HAVE UPDATED IX1, IX2, AND IX3 -- DEFINE SOME VARIABLES FOR LATER
!--USE

90 dtx1 = xt1(ix1)-xt1(ix1+1)
dtx2 = xt2(ix2)-xt2(ix2+1)
dtx3 = xt3(ix3)-xt3(ix3+1)
!99      IF (DTX1.EQ.0.D0.OR.DTX2.EQ.0.D0.OR.DTX3.EQ.0.D0) THEN
!99 CALL ERROR(2012)
!99      ELSE
dx1 = x1-xt1(ix1)
dx2 = x2-xt2(ix2)
dx3 = x3-xt3(ix3)
sz1 = (yt(ix1,ix2,ix3)    -yt(ix1,ix2,ix3+1))/dtx3
sz2 = (yt(ix1+1,ix2,ix3)  -yt(ix1+1,ix2,ix3+1))/dtx3
sz3 = (yt(ix1+1,ix2+1,ix3)-yt(ix1+1,ix2+1,ix3+1))/dtx3
sz4 = (yt(ix1,ix2+1,ix3)  -yt(ix1,ix2+1,ix3+1))/dtx3
sy1 = (yt(ix1,ix2,ix3)    -yt(ix1+1,ix2,ix3))/dtx1
sy2 = (yt(ix1,ix2+1,ix3)  -yt(ix1+1,ix2+1,ix3))/dtx1
sy3 = (yt(ix1,ix2+1,ix3+1)-yt(ix1+1,ix2+1,ix3+1))/dtx1
sy4 = (yt(ix1,ix2,ix3+1)  -yt(ix1+1,ix2,ix3+1))/dtx1
sx1 = (yt(ix1,ix2,ix3)    -yt(ix1,ix2+1,ix3))/dtx2
sx2 = (yt(ix1+1,ix2,ix3)  -yt(ix1+1,ix2+1,ix3))/dtx2
sx3 = (yt(ix1+1,ix2,ix3+1)-yt(ix1+1,ix2+1,ix3+1))/dtx2
sx4 = (yt(ix1,ix2,ix3+1)  -yt(ix1,ix2+1,ix3+1))/dtx2
z1t = yt(ix1,ix2,ix3)    +sz1*dx3
z2t = yt(ix1+1,ix2,ix3)  +sz2*dx3
z3t = yt(ix1+1,ix2+1,ix3)+sz3*dx3
z4t = yt(ix1,ix2+1,ix3)  +sz4*dx3
y1t = yt(ix1,ix2,ix3)    +sy1*dx1
y2t = yt(ix1,ix2+1,ix3)  +sy2*dx1
y3t = yt(ix1,ix2+1,ix3+1)+sy3*dx1
y4t = yt(ix1,ix2,ix3+1)  +sy4*dx1
x1t = yt(ix1,ix2,ix3)    +sx1*dx2
x2t = yt(ix1+1,ix2,ix3)  +sx2*dx2
x3t = yt(ix1+1,ix2,ix3+1)+sx3*dx2
x4t = yt(ix1,ix2,ix3+1)  +sx4*dx2
y1n = z1t+(z1t-z2t)/dtx1*dx1
y3n = z4t+(z4t-z3t)/dtx1*dx1
x1n = x1t+(x1t-x4t)/dtx3*dx3
x3n = x2t+(x2t-x3t)/dtx3*dx3
z1n = y1t+(y1t-y2t)/dtx2*dx2
z3n = y4t+(y4t-y3t)/dtx2*dx2
ss1 = (sz1-sz2)/dtx1
ss3 = (sz4-sz3)/dtx1
sy1 = sz1+ss1*dx1
sy3 = sz4+ss3*dx1

!--COMPUTE DERIVATIVES OF DATA WITH RESPECT TO INDEPENDENT VARIABLES

yd1 = (x1n-x3n)/dtx1
yd2 = (y1n-y3n)/dtx2
yd3 = (z1n-z3n)/dtx3

!--INTERPOLATE

y = y1n+yd2*dx2
!99      END IF
RETURN
END SUBROUTINE tblp3
!#######################################################################

SUBROUTINE tblp4(x1,x2,x3,x4,ix1,ix2,ix3,ix4,xt1,xt2,xt3,xt4,  &
    mx1,mx2,mx3,mx4,yt,y,sl1,sl2,sl3,sl4)

!#######################################################################

!     TBLP4 IS A GENERAL FOUR-DIMENSIONAL TABLE LOOK-UP WITH POINTERS
!     TO ENHANCE SPEED


!     INPUTS:

!       X1  - INDEPENDENT VARIABLE 1
!       X2  - INDEPENDENT VARIABLE 2
!       X3  - INDEPENDENT VARIABLE 3
!       X4  - INDEPENDENT VARIABLE 4
!       IX1 - POINTER TO LAST TABLE LOOKUP POSITION FOR X1
!       IX2 - POINTER TO LAST TABLE LOOKUP POSITION FOR X2
!       IX3 - POINTER TO LAST TABLE LOOKUP POSITION FOR X3
!       IX4 - POINTER TO LAST TABLE LOOKUP POSITION FOR X4
!       XT1 - ARRAY OF INDEPENDENT VARIABLE 1 VALUES (MX1 VALUES)
!       XT2 - ARRAY OF INDEPENDENT VARIABLE 2 VALUES (MX2 VALUES)
!       XT3 - ARRAY OF INDEPENDENT VARIABLE 3 VALUES (MX3 VALUES)
!       XT4 - ARRAY OF INDEPENDENT VARIABLE 4 VALUES (MX4 VALUES)
!       YT  - MATRIX OF DEPENDENT VARIABLES (MX1*MX2*MX3*MX4 VALUES)
!       MX1 - MAXIMUM OF INDEPENDENT VARIABLE 1 VALUES IN TABLE
!       MX2 - MAXIMUM OF INDEPENDENT VARIABLE 2 VALUES IN TABLE
!       MX3 - MAXIMUM OF INDEPENDENT VARIABLE 3 VALUES IN TABLE
!       MX4 - MAXIMUM OF INDEPENDENT VARIABLE 4 VALUES IN TABLE


!     OUTPUTS:

!       Y - DEPENDENT VALUE FROM TABLE LOOK UP

!#######################################################################


REAL, INTENT(IN OUT)                     :: x1
REAL, INTENT(IN OUT)                     :: x2
REAL, INTENT(IN OUT)                     :: x3
REAL, INTENT(IN)                         :: x4
INTEGER, INTENT(IN OUT)                  :: ix1
INTEGER, INTENT(IN OUT)                  :: ix2
INTEGER, INTENT(IN OUT)                  :: ix3
INTEGER, INTENT(OUT)                     :: ix4
REAL, INTENT(IN OUT)                     :: xt1(mx1)
REAL, INTENT(IN OUT)                     :: xt2(mx2)
REAL, INTENT(IN OUT)                     :: xt3(mx3)
REAL, INTENT(IN)                         :: xt4(mx4)
INTEGER, INTENT(IN OUT)                  :: mx1
INTEGER, INTENT(IN OUT)                  :: mx2
INTEGER, INTENT(IN OUT)                  :: mx3
INTEGER, INTENT(IN)                      :: mx4
REAL, INTENT(IN OUT)                     :: yt(mx1,mx2,mx3,mx4)
REAL, INTENT(OUT)                        :: y
REAL, INTENT(OUT)                        :: sl1
REAL, INTENT(OUT)                        :: sl2
REAL, INTENT(OUT)                        :: sl3
REAL, INTENT(OUT)                        :: sl4

LOGICAL :: ascnd

!--FIND POINTER FOR INDEPENDENT VARIABLE 4

ascnd = xt4(mx4) >= xt4(1)
ix4 = MAX0(1,MIN0(ix4,mx4))
IF (x4 > xt4(ix4).EQV.ascnd) THEN
  
!--SEARCH UP FOR INDEPENDENT VARIABLE 4
  
  DO   i=ix4+1,mx4
    IF (xt4(i) > x4.EQV.ascnd) THEN
      ix4 = i-1
      GO TO 30
    END IF
  END DO
  ix4 = mx4-1
  
!--SEARCH DOWN FOR INDEPENDENT VARIABLE 4
  
ELSE
  DO   i=ix4-1,1,-1
    IF (xt4(i) <= x4.EQV.ascnd) THEN
      ix4 = i
      GO TO 30
    END IF
  END DO
  ix4 = 1
END IF

!--NOW HAVE UPDATED IX1, IX2, AND IX3 -- DEFINE SOME VARIABLES FOR LATER
!--USE

30 dtx4 = xt4(ix4+1)-xt4(ix4)
!99      IF (DTX4.EQ.0.0D0) THEN
!99        CALL ERROR(3012)
!99      ELSE
dx4  = x4-xt4(ix4)
CALL tblp3(x1,x2,x3,ix1,ix2,ix3,xt1,xt2,xt3,mx1,mx2,mx3,  &
    yt(1,1,1,ix4),y4,dy1a,dy2a,dy3a)
CALL tblp3(x1,x2,x3,ix1,ix2,ix3,xt1,xt2,xt3,mx1,mx2,mx3,  &
    yt(1,1,1,ix4+1),y4n,dy1b,dy2b,dy3b)
yd = (y4n-y4)/dtx4
y  = y4+yd*dx4
!99      END IF

!--COMPUTE SLOPES

pct = dx4/dtx4
sl1 = dy1a+pct*(dy1b-dy1a)
sl2 = dy2a+pct*(dy2b-dy2a)
sl3 = dy3a+pct*(dy3b-dy3a)
sl4 = yd
RETURN
END SUBROUTINE tblp4

!************************************************************************
!************* DOUBLE PRECISION MATRIX UTILITIES ************************
!************************************************************************

SUBROUTINE dmatdbl (d,s,n1,n2)

!*** DOUBLE PRECISION D(N1*N2) IS OBTAINED FROM SINGLE PRECISION S(N1*N2)


DOUBLE PRECISION, INTENT(OUT)            :: d(*)
REAL, INTENT(IN OUT)                     :: s(*)
INTEGER, INTENT(IN)                      :: n1
INTEGER, INTENT(IN)                      :: n2



l=n1*n2
DO  i=1,l
  d(i)=DBLE(s(i))
END DO
RETURN
END SUBROUTINE dmatdbl

SUBROUTINE dmatsgl (s,d,n1,n2)

!*** SINGLE PRECISION S(N1*N2) IS OBTAINED FROM DOUBLE PRECISION S(N1*N2)


REAL, INTENT(OUT)                        :: s(*)
DOUBLE PRECISION, INTENT(IN OUT)         :: d(*)
INTEGER, INTENT(IN)                      :: n1
INTEGER, INTENT(IN)                      :: n2



l=n1*n2
DO  i=1,l
  s(i)=SNGL(d(i))
END DO
RETURN
END SUBROUTINE dmatsgl

SUBROUTINE dmatadd (c,a,b,n1,n2)

!*** DOUBLE PRECISION VERSION OF MATADD
!*** C(N1*N2) IS OBTAINED FROM ADDING A(N1*N2) TO B(N1*N2)



DOUBLE PRECISION, INTENT(OUT)            :: c(*)
DOUBLE PRECISION, INTENT(IN)             :: a(*)
DOUBLE PRECISION, INTENT(IN)             :: b(*)
INTEGER, INTENT(IN)                      :: n1
INTEGER, INTENT(IN)                      :: n2


DO  j=1,n2
  DO  i=1,n1
    k=n2*(j-1)+i
    c(k)=a(k)+b(k)
  END DO
END DO
RETURN
END SUBROUTINE dmatadd

SUBROUTINE dmatsub (c,a,b,n1,n2)

!*** DOUBLE PRECISION VERSION OF MATSUB
!*** C(N1*N2) IS OBTAINED FROM SUBSTRACTING B(N1*N2) FROM A(N1*N2)



DOUBLE PRECISION, INTENT(OUT)            :: c(*)
DOUBLE PRECISION, INTENT(IN)             :: a(*)
DOUBLE PRECISION, INTENT(IN)             :: b(*)
INTEGER, INTENT(IN)                      :: n1
INTEGER, INTENT(IN)                      :: n2


DO  j=1,n2
  DO  i=1,n1
    k=n2*(j-1)+i
    c(k)=a(k)-b(k)
  END DO
END DO
RETURN
END SUBROUTINE dmatsub

SUBROUTINE dmatabs (d,v,n)

!*** DOUBLE PRECISION VERSION OF MATABS
!*** D IS THE ABSOLUTE VALUE OF VECTOR V(N*1)



DOUBLE PRECISION, INTENT(OUT)            :: d
DOUBLE PRECISION, INTENT(IN)             :: v(*)
INTEGER, INTENT(IN)                      :: n
DOUBLE PRECISION :: dv

dv=0.
DO  i=1,n
  dv=dv+v(i)*v(i)
END DO
d=SQRT(dv)
RETURN
END SUBROUTINE dmatabs

SUBROUTINE dmatmul (c,a,b,n1,n2,n3)

!*** DOUBLE PRECISION VERSION OF MATMUL
!*** C(N1*N2) IS OBTAINED FROM MULTIPLYING A(N1*N2) BY B(N2*N3)



DOUBLE PRECISION, INTENT(OUT)            :: c(*)
DOUBLE PRECISION, INTENT(IN)             :: a(*)
DOUBLE PRECISION, INTENT(IN)             :: b(*)
INTEGER, INTENT(IN)                      :: n1
INTEGER, INTENT(IN)                      :: n2
INTEGER, INTENT(IN)                      :: n3


DO i=1,n1
  DO k=1,n3
    n=n1*(k-1)+i
    c(n)=0.0
    DO j=1,n2
      l=n1*(j-1)+i
      m=n2*(k-1)+j
      c(n)=c(n)+a(l)*b(m)
    END DO
  END DO
END DO
RETURN
END SUBROUTINE dmatmul

SUBROUTINE dmattra (c,a,n1,n2)

!*** DOUBLE PRECISION VERSION OF MATTRA
!*** C(N2*N1) IS THE TRANSPOSE OF A(N1*N2)



DOUBLE PRECISION, INTENT(OUT)            :: c(n2,n1)
DOUBLE PRECISION, INTENT(IN)             :: a(n1,n2)
INTEGER, INTENT(IN)                      :: n1
INTEGER, INTENT(IN)                      :: n2


DO  j=1,n2
  DO  i=1,n1
    c(j,i)=a(i,j)
  END DO
END DO
RETURN
END SUBROUTINE dmattra

!          SUBROUTINE DMATINV

!          PURPOSE
!             COMPUTE THE INVERSE OF A NON-SINGLAR MATRIX
!             DOUBLE PRECISION VERSION OF MATINV
!          USAGE
!             CALL DMATINV(AI,A,D,N)

!          DESCRIPTION OF PARAMETERS
!             A - NAME OF INPUT MATRIX
!             AI- NAME OF OUTPUT MATRIX (MAY BE THE SAME AS A)
!             D - DETERMINANT OF A
!             N - NUMBER OF ROWS AND COLUMNS OF A

!          REMARKS
!             ALL MATRICES ARE STORED AS GENERAL MATRICES

!        SET UP TO CALL GAUSS-JORDAN INVERSION ROUTINE.

!     ..................................................................

SUBROUTINE dmatinv(ai,a,d,n)

DOUBLE PRECISION, INTENT(OUT)            :: ai(*)
DOUBLE PRECISION, INTENT(IN)             :: a(*)
DOUBLE PRECISION, INTENT(OUT)            :: d
INTEGER, INTENT(IN)                      :: n
IMPLICIT DOUBLE PRECISION (a-h,o-z)


!     SET UP TO CALL ROUTINE THAT ACTUALLY DOES THE CONVERSION

mx = n
m = n
epsil = 1.d-10

!         COPY A MATRIX INTO AI MATRIX

n2=n*n
DO  i = 1, n2
  ai(i)=a(i)
END DO

CALL dmatfs(epsil,mx,m,ai,ier,det)
d=det
IF(ier == 0) RETURN
d=0.d0
PRINT *,' MATRIX IS SINGULAR '
x=-2
y=SQRT(x)**2
RETURN
END SUBROUTINE dmatinv

SUBROUTINE dmatfs(epsil,mx,m,a,ier,det)

!      DMATFS IS A DOUBLE-PRECISION MATRIX INVERSION SUBROUTINE
!     WRITTEN IN FORTRAN IV  - VARIABLE DIMENSIONING IS USED
!     OF MAX DIMENSIONS OF MATRIX IN CSLLING PROGRAM MUST BE SAME
!     VALUE AS PASSED ON BY CALLING SEQUENCE AS MX.
!     MATRIX CAN BE OF GENERAL FORM. METHOD IS GAUSS-JORDAN.


DOUBLE PRECISION, INTENT(IN OUT)         :: epsil
INTEGER, INTENT(IN OUT)                  :: mx
INTEGER, INTENT(IN)                      :: m
DOUBLE PRECISION, INTENT(IN OUT)         :: a(mx,mx)
INTEGER, INTENT(OUT)                     :: ier
DOUBLE PRECISION, INTENT(OUT)            :: det
IMPLICIT DOUBLE PRECISION (a-h,o-z)
INTEGER :: i,j,k,kp,jf,ibeta,l,ig,iphi, ifkp,lda,igkp

DIMENSION jf(30),ig(30),l(30),ibeta(30),temp(30)

!     INITIALIZE AND LOGICAL OPERATIONS ON COLUMN K, PIVOT COL.
det=1.0D0
ier =0
DO  kp =1,m
  jf(kp) = 0
  ibeta(kp) = 0
  l(kp)=kp
  ig(kp)=kp
END DO
!     TRANSFORM BY COLUMNS, K
DO  k = 1, m
!     FIND LARGEST ELEMENT IN COL K WITH IBETA STILL ZERO
  rmu=0.0D0
  DO  i = 1,m
    IF (ibeta(i) < 0) THEN
      GO TO  1000
    ELSE IF (ibeta(i) == 0) THEN
      GO TO    10
    ELSE
      GO TO    14
    END IF
    10 IF ( ABS(a(i,k)) -  ABS(rmu) > 0.0) THEN
      GO TO    12
    ELSE
      GO TO    14
    END IF
!     LABEL ROW WITH LARGEST ELEMENT (RMU) AS IPHI
    12 rmu = a(i,k)
    iphi = i
  END DO
!     IF ELEMENT RMU IS TOO SMALL,MATRIX IS SINGULAR-CALL EXIT
  15 IF ( ABS(rmu) - epsil > 0.0) THEN
    GO TO    18
  END IF
  16   ier =1
  RETURN
  18 det = rmu*det
!     SETF(K) WITH INDEX OF ROW, IBETA OF ROW NOW 1
  jf(k) = iphi
  ibeta(iphi) = 1
!     ARITHMETIC OPERATIONS ON PIVOT COLUMN,K
!     FIRST SAVE ENTIRE COLUMN IN PRESENT FORM
  20 DO  i=1,m
    temp(i) = a(i,k)
  END DO
!     TRANSFORM COLUMN
  a(iphi,k) = 1.0D0/rmu
  DO  i=1,m
    IF (i - iphi == 0) THEN
      GO TO    23
    END IF
    22 a(i,k) = -a(i,k)/rmu
  END DO
!     LOGICAL OPERATIONS ON OTHER COLUMNS    PANEL IV
!     NOTE WE USE ORIG SAVED VALUES FROM PIVOT COLUMN,K
  DO  j =1,m
!     TEST TO SKIP COL K, ALREADY TRANSFORMED
    IF (j-k == 0) THEN
      GO TO    31
    END IF
!     NOT K   IF VALUE IN ROW IPHI IS 0 SKIP COLUMN
    25 IF (a(iphi,j) == 0.0) THEN
      GO TO    31
    END IF
!     NOT ZERO
    26 alpha = a(iphi,j)
    a(iphi,j) = alpha/rmu
!     TRANSFORM IN ONE COL,J, BY ROW INDICES
    DO  i =1,m
!     TEST IF ELEMENT IS IN ROW WITH PIVOT ELEMENT
      IF (i-iphi == 0) THEN
        GO TO    30
      END IF
!     NO
      28 a(i,j) = a(i,j) - alpha*temp(i) /rmu
!     YES
    END DO
  END DO
END DO
!     PERMUTATIONS OF ROWS AND COLUMNS
mm=m-1
DO  kp=1,mm
!     PERMUTATION OF ROWS
  IF(ig(kp)-jf(kp) == 0) THEN
    GO TO    55
  END IF
  50 ifkp = jf(kp)
  lda = l(ifkp)
  DO  j =1,m
    temp(j) = a(kp,j)
    a(kp,j) = a(lda,j)
    a(lda,j) = temp(j)
    det = -det
  END DO
!     PERMUTATION OF COLUMNS
  igkp = ig(kp)
  DO  i = 1,m
    temp(i) = a(i,ifkp)
    a(i,ifkp) = a(i,igkp)
    a(i,igkp) = temp(i)
!     RESET INDICES
    l(ifkp) = k
    l(igkp) = lda
    ig(lda) = igkp
    ig(kp) = ifkp
  END DO
END DO
RETURN
1000  WRITE (6,1001)
STOP
1001 FORMAT (22H error, ibeta negative )
!     INVERTED MATRIX IS IN CELLS ORIGINALLY OCCUPIED BY MATRIX
!     DETERMINANT IS IN CELL DET (NOT RETURNED)
60 RETURN
END SUBROUTINE dmatfs
!**********************************************************************

SUBROUTINE us76(rho,press,tempk,balt)
!**********************************************************************
!*** *
!*** * US Standard Atmosphere 1976
!*** *
!*** * This module performs the following functions:
!*** *  Calculates the atmospheric properties
!*** *
!*** * Argument Output:
!*** *     RHO=Air density - kg/m^3
!*** *     PRESS= Air static pressure - Pa
!*** *     TEMPK= Air temperature - degKelvin
!*** * Argument Input:
!*** *     BALT= Geometrical altitude above S.L. - m
!*** *
!*** * MODIFICATION HISTORY
!*** * 000204 Created by Peter Zipfel
!*** *
!*** ******************************************************************


REAL, INTENT(OUT)                        :: rho
REAL, INTENT(OUT)                        :: press
REAL, INTENT(OUT)                        :: tempk
REAL, INTENT(IN)                         :: balt
REAL, PARAMETER :: rearth = 6369.0
REAL, PARAMETER :: gmr = 34.163195
REAL, PARAMETER :: rhosl=1.22500
REAL, PARAMETER :: pressl=101325.
REAL, PARAMETER :: tempksl=288.15
INTEGER, PARAMETER :: ntab=8

DIMENSION htab(8),ttab(8),ptab(8),gtab(8)

DATA htab/0.0, 11.0, 20.0, 32.0, 47.0, 51.0, 71.0, 84.852/
DATA ttab /288.15, 216.65, 216.65, 228.65, 270.65, 270.65, 214.65, 186.946/
DATA ptab /1.0, 2.233611E-1, 5.403295E-2, 8.5666784E-3, 1.0945601E-3,  &
    6.6063531E-4, 3.9046834E-5, 3.68501E-6/
DATA gtab /-6.5, 0.0, 1.0, 2.8, 0.0, -2.8, -2.0, 0.0/

alt=balt/1000.
h=alt*rearth/(alt+rearth)! convert geometric to geopotential altitude

i=1
j=ntab                   ! setting up for binary search
DO
  k=(i+j)/2              ! integer division
  IF (h < htab(k)) THEN
    j=k
  ELSE
    i=k
  END IF
  IF (j <= (i+1)) EXIT
END DO

tgrad=gtab(i)            ! i will be in 1...NTAB-1
tbase=ttab(i)
deltah=h-htab(i)
tlocal=tbase+tgrad*deltah
theta=tlocal/ttab(1)     ! temperature ratio

IF (tgrad == 0.0) THEN   ! pressure ratio
  delta=ptab(i)*EXP(-gmr*deltah/tbase)
ELSE
  delta=ptab(i)*(tbase/tlocal)**(gmr/tgrad)
END IF

sigma=delta/theta        ! density ratio

rho=rhosl*sigma
press=pressl*delta
tempk=tempksl*theta
RETURN
END SUBROUTINE us76

!***********************************************************************
!************************ CAD UTILITIES ********************************
!***********************************************************************

SUBROUTINE cadelp(ama,ami,phi,aa)

!*** MAJOR, MINOR AXES OF ELLIPSE AND ROTATION ANGLE
!    FROM THE SYMMETRICAL POS SEMI-DEFINITE AA(2X2) MATRIX

!      Coordinate axes orientation:

!          ^ 1-axis
!          |
!          |---> 2-axis

!  ARGUMENT OUTPUT:

!          AMA   =Major semi-axis
!          AMI   =Minor semi=axis
!          PHIEV =Angle of major axis wrt first coord axis

!  ARGUMENT INPUT:

!          AA(2x2) =Real symmetrical pos semi-definite matrix



REAL, INTENT(OUT)                        :: ama
REAL, INTENT(OUT)                        :: ami
REAL, INTENT(OUT)                        :: phi
REAL, INTENT(IN)                         :: aa(2,2)
DIMENSION  x1v(2),x2v(2)

!*** MAJOR AND MINOR SEMI-AXES (EIGENVALUES OF SUBMATRIX)

a11=aa(1,1)
a22=aa(2,2)
a12=aa(1,2)
a1133=a11+a22
aq1122=a1133**2
dum1=aq1122-4.*(a11*a22-a12**2)
IF(dum1 >= 0.) dum2=SQRT(dum1)
ama=(a1133+dum2)/2.
ami=(a1133-dum2)/2.
IF(ama == ami) RETURN

!*** PRINCIPAL AXES IN ORIGINAL(V)-COORDINATES (EIGENVECTORS)

IF(a11-ama /= 0.) THEN
  dum1=-a12/(a11-ama)
  ak1=SQRT(1./(1.+dum1**2))
  x1v(1)=dum1*ak1
  x1v(2)=ak1
  dum=dum1*ak1
  IF(ABS(dum) > 1.) dum=SIGN(1.,dum)
  phi=ACOS(dum)
ELSE
  dum1=-a12/(a22-ama)
  ak1=SQRT(1./(1.+dum1**2))
  x1v(1)=ak1
  x1v(2)=dum1*ak1
  IF(ABS(ak1) > 1.) ak1=SIGN(1.,ak1)
  phi=ACOS(ak1)
END IF
IF(a11-ami /= 0.) THEN
  dum2=-a12/(a11-ami)
  ak2=SQRT(1./(1.+dum2**2))
  x2v(1)=dum2*ak2
  x2v(2)=ak2
ELSE
  dum2=-a12/(a22-ami)
  ak2=SQRT(1./(1.+dum2**2))
  x2v(1)=ak2
  x2v(2)=dum2*ak2
END IF

RETURN
END SUBROUTINE cadelp
!**********************************************************************

SUBROUTINE cadorb(e,de,perg,ainc,antii,alngn,omeg,anu,altii, stii,vtii)
!**********************************************************************
!*** *
!*** * Orbital elements from inerftial position and velocity
!*** *
!*** * Argument Output:
!*** *          E(3) =Orbit eccentricity vector - ND
!*** *          DE =Orbit eccentricity - ND
!*** *          PERG =Radius of perigee - m
!*** *          AINC =Inclination of orbit - rad
!*** *          ANTII(3) =Node vector - m^2/s
!*** *          ALNGN =Longitude of the ascending node - rad
!*** *          OMEG =Argument of periapsis - rad
!*** *          ANU =True anomaly - rad
!*** *          ALTII(3) =Angular momentum vector of orbit - m^2/s
!*** * Argument Input:
!*** *          STII(3) =Position of T wrt I in inertial coord - m
!*** *          VTII(3) =Inertial Velocity of T in inertial coor - m/s
!*** *
!*** * MODIFICATION HISTORY
!*** * 840331 Created by Jim Williford
!*** *
!**********************************************************************


REAL, INTENT(IN OUT)                     :: e(3)
REAL, INTENT(IN OUT)                     :: de
REAL, INTENT(OUT)                        :: perg
REAL, INTENT(OUT)                        :: ainc
REAL, INTENT(OUT)                        :: antii(3)
REAL, INTENT(OUT)                        :: alngn
REAL, INTENT(OUT)                        :: omeg
REAL, INTENT(OUT)                        :: anu
REAL, INTENT(IN)                         :: altii(3)
REAL, INTENT(IN OUT)                     :: stii(3)
REAL, INTENT(IN OUT)                     :: vtii(3)
DOUBLE PRECISION :: dantid,ded,dum2d,arg2d,domegd
COMMON c(3510)

DIMENSION  sstii(3,3), dumv1(3),dumv2(3),dumv3(3)

!*** INPUT FROM OTHER MODULES

EQUIVALENCE (c(0051),rearth)
EQUIVALENCE (c(0052),crad)
EQUIVALENCE (c(0057),amu)

!*** TARGET ANGULAR MOMENTUM [ALTII] AND NODE [ANTII] VECTORS

CALL matsks(sstii,stii)
CALL matmul(altii,sstii,vtii,3,3,1)
CALL matabs(dalti,altii,3)

antii(1)=-altii(2)
antii(2)=altii(1)
antii(3)=0.
CALL matabs(danti,antii,3)

!*** ORBIT ECCENTRICITY

CALL matabs(dti,stii,3)
CALL matsca(dum1,stii,vtii,3)
CALL matcon(dumv1,dum1,vtii,3,1)
CALL matabs(dvti,vtii,3)
dum2=dvti**2.-amu/dti
CALL matcon(dumv2,dum2,stii,3,1)
CALL matsub(dumv3,dumv2,dumv1,3,1)
dum2=1./amu
CALL matcon(e,dum2,dumv3,3,1)  !!
CALL matabs(de,e,3)

!*** TARGET PERIGEE (ABOVE GROUND), ORBIT INCLINATION, AND LONGITUDE
!    OF ASCENDING NODE

alatrc=(dalti**2.)/amu
peri=alatrc/(1+de)
perg=peri-rearth

ainc=ACOS(altii(3)/dalti)

dalngn=ACOS(antii(1)/(SQRT(antii(1)**2.+antii(2)**2.)))

!*** ARGUMENT OF THE PERIGEE AND TRUE ANOMALY

CALL matsca(dum2,antii,e,3)
!*** CONVERSION TO DOUBLE PRECISION
dantid=DBLE(danti)
ded=DBLE(de)
dum2d=DBLE(dum2)

arg2d=dum2d/(dantid*ded)
IF(arg2d > 1.) arg2d=1.
IF(arg2d < -1.) arg2d=-1.
domegd=DACOS(arg2d)
!*** CONVERSION TO SINGLE PRECISION
domeg=SNGL(domegd)

CALL matsca(dum3,e,stii,3)
arg=dum3/(de*dti)
IF(arg > 1.) arg=1.
IF(arg < -1.) arg=-1.
danu=ACOS(arg)


!*** QUADRANT ACCOUNTING

twopi=360./crad
CALL matsca(dum4,stii,vtii,3)

IF(dum4 >= 0.) THEN
  anu=danu
ELSE
  anu=twopi-danu
END IF

IF(e(3) >= 0.) THEN
  omeg=domeg
ELSE
  omeg=twopi-domeg
END IF

IF(antii(2) > 0.) THEN
  alngn=dalngn
ELSE
  alngn=twopi-dalngn
END IF

RETURN
END SUBROUTINE cadorb
!******************************************************************

SUBROUTINE cadtgi84(tgi,blon,blat,balt,alon0)
!******************************************************************
!*** * Calculates geographic (geocentric) wrt inertial T.M.
!*** *  using the WGS 84 reference ellipsoid
!*** * Reference: Britting,K.R."Inertial Navigation Systems Analysis",
!*** * Wiley, 1971
!*** *
!*** * Argument ouput: TGI(3x3) =Geographic wrt inertial T.M.- ND
!*** * Argument input: BLON =Geodetic longitude - rad
!*** *                 BLAT =Geodetic latitude - rad
!*** *                 BALT =Height above ellipse - m
!*** *                 ALON0=Greenwich celestial longitude at t=0 - rad
!*** *
!*** * MODIFICATION HISTORY
!*** * 990413 Created by Peter Zipfel
!*** * 990625 Use of deflection DD instead of D0, PZi
!*** *
!*** **************************************************************

REAL, INTENT(IN OUT)                     :: tgi(3,3)
REAL, INTENT(IN)                         :: blon
REAL, INTENT(IN OUT)                     :: blat
REAL, INTENT(IN)                         :: balt
REAL, INTENT(IN)                         :: alon0
COMMON c(3510)

DIMENSION  tgd(3,3),tdi(3,3)

!*** INPUT FROM EXEC

EQUIVALENCE (c(0058),weii3)
EQUIVALENCE (c(2000),t)

! WEII3 =Earth's mean angular velocity (WGS84) - rad/s

!*** WGS 84 Parameters

REAL, PARAMETER :: ee = 0.003352811
REAL, PARAMETER :: ael = 6378137.

! EE =flattening of earth's ellipsoid - ND
! AEL =Major axis of earth's ellipsoid - m

!*** Celestial longitude of vehicle at time T

blonc=alon0+weii3*t+blon

!*** T.M. of geodetic coord wrt inertial coord., TDI(3x3)

tdi(1,3)=COS(blat)
tdi(3,3)=-SIN(blat)
tdi(2,2)=COS(blonc)
tdi(2,1)=-SIN(blonc)
tdi(1,1)=tdi(3,3)*tdi(2,2)
tdi(1,2)=-tdi(3,3)*tdi(2,1)
tdi(3,1)=-tdi(1,3)*tdi(2,2)
tdi(3,2)=tdi(1,3)*tdi(2,1)
tdi(2,3)=0.

!*** T.M. of geographic (geocentric) wrt geodetic coord., TGD(3x3)

CALL matuni(tgd,3)
r0=ael*(1.-ee*(1.-COS(2.*blat))/2.+5.*ee**2*(1.-COS(4.*blat))/16.) !Eq 4-21
dd=ee*SIN(2.*blat)*(1.-ee/2.-balt/r0) !Eq 4-15
tgd(1,1)=COS(dd)
tgd(3,3)=tgd(1,1)
tgd(3,1)=SIN(dd)
tgd(1,3)=-tgd(3,1)

!*** T.M. of geographic (geocentric) wrt inertial coord., TGI(3x3)

CALL matmul(tgi,tgd,tdi,3,3,3)

RETURN
END SUBROUTINE cadtgi84
!******************************************************************

SUBROUTINE cadgeo84(blon,blat,balt,sbii,alon0)
!******************************************************************
!*** * Calculates geodetic longitude, latitude, and altitude
!*** *  using the WGS 84 reference ellipsoid
!*** * Reference: Britting,K.R."Inertial Navigation Systems Analysis",
!*** * Wiley, 1971
!*** *
!*** * Argument output: BLON =Geodetic longitude - rad
!*** *                  BLAT =Geodetic latitude - rad
!*** *      BALT =Altitude above ellipsoid - m
!*** * Argument input:  SBII(3x1) =Inertial vehicle position - m
!*** *                  ALON0=Greenwich celestial longitude at t=0 - rad
!*** *
!*** *
!*** * MODIFICATION HISTORY
!*** * 990414 Created by Peter Zipfel
!*** * 990625 Use of deflection DD instead of D0, PZi
!*** **************************************************************


REAL, INTENT(OUT)                        :: blon
REAL, INTENT(OUT)                        :: blat
REAL, INTENT(OUT)                        :: balt
REAL, INTENT(IN)                         :: sbii(3)
REAL, INTENT(IN)                         :: alon0
COMMON c(3510)



!*** INPUT FROM EXEC

EQUIVALENCE (c(0058),weii3)
EQUIVALENCE (c(2000),t)

! WEII3 =Earth's mean angular velocity (WGS84) - rad/s

!*** WGS 84 Parameters

REAL, PARAMETER :: ee = 0.003352811
REAL, PARAMETER :: ael = 6378137.

! EE =flattening of earth's ellipsoid - ND
! AEL =Major axis of earth's ellipsoid - m

!*** Initialize geodetic latitude using geocentric latitude

CALL matabs(dbi,sbii,3)
blatg=ASIN(sbii(3)/dbi)
blat=blatg
icount=0

!*** Iterative loop to caculate geodetic latitude and altitude

10 CONTINUE

blm=blat
r0=ael*(1.-ee*(1.-COS(2.*blm))/2.+5.*ee**2*(1.-COS(4.*blm))/16.) !Eq 4-21
balt=dbi-r0
dd=ee*SIN(2.*blm)*(1.-ee/2.-balt/r0) !Eq 4-15
blat=blatg+dd
icount=icount+1
IF(icount > 500) THEN
!---  TYPE*,' *** STOP: 500 iterations in sbr CADGEO84 ***'
  PRINT *,' *** STOP: 500 iterations in sbr CADGEO84 ***'
  STOP
END IF

IF(ABS(blat-blm) > 1E-7) GO TO 10

!*** Geodetic (identical to geographic(geocentric)) longitude

blon=ASIN(sbii(2)/SQRT(sbii(1)**2+sbii(2)**2))-weii3*t-alon0

RETURN
END SUBROUTINE cadgeo84


!******************************************************************

SUBROUTINE cadine84(sbii,blon,blat,balt,alon0)
!******************************************************************
!*** * Calculates inertial position vector from geodetic longitude,
!*** *  latitude, and altitude using the WGS 84 reference ellipsoid
!*** * Reference: Britting,K.R."Inertial Navigation Systems Analysis"
!*** * pp.45-49, Wiley, 1971
!*** *
!*** * Argument ouput: SBII(3x1) =Inertial vehicle position - m
!*** * Argument input: BLON =Geodetic longitude - rad
!*** *                 BLAT =Geodetic latitude - rad
!*** *                 BALT =Altitude above ellipsoid - m
!*** *                 ALON0=Greenwich celestial longitude at t=0 - rad
!*** *
!*** * MODIFICATION HISTORY
!*** * 990413 Created by Peter Zipfel
!*** * 990625 Use of deflection DD instead of D0, PZi
!*** *
!*** **************************************************************


REAL, INTENT(OUT)                        :: sbii(3)
REAL, INTENT(IN)                         :: blon
REAL, INTENT(IN OUT)                     :: blat
REAL, INTENT(IN)                         :: balt
REAL, INTENT(IN)                         :: alon0
COMMON c(3510)

DIMENSION  sbid(3)

!*** INPUT FROM EXEC

EQUIVALENCE (c(0058),weii3)
EQUIVALENCE (c(2000),t)

! WEII3 =Earth's mean angular velocity (WGS84) - rad/s

!*** WGS 84 Parameters

REAL, PARAMETER :: ee = 0.003352811
REAL, PARAMETER :: ael = 6378137.

! EE =flattening of earth's ellipsoid - ND
! AEL =Major axis of earth's ellipsoid - m

!*** Deflection of the normal, DD, and length of earth's radius to ellipse, R0

r0=ael*(1.-ee*(1.-COS(2.*blat))/2.+5.*ee**2*(1.-COS(4.*blat))/16.) !Eq 4-21
dd=ee*SIN(2.*blat)*(1.-ee/2.-balt/r0) !Eq 4-15

!*** Vehicles's displacement vector from earth's center SBID(3x1) in geodetic coord.

dbi=r0+balt
sbid(1)=-dbi*SIN(dd)
sbid(2)=0.
sbid(3)=-dbi*COS(dd)

!*** Celestial longitude of vehicle at time T

blonc=alon0+weii3*t+blon

!*** Vehicle's displacement vector in inertial coord. SBII(3x1)=TID(3x3)xSBID(3x3)

slat=SIN(blat)
clat=COS(blat)
slon=SIN(blonc)
clon=COS(blonc)
sbii(1)=-slat*clon*sbid(1)-clat*clon*sbid(3)
sbii(2)=-slat*slon*sbid(1)-clat*slon*sbid(3)
sbii(3)=clat*sbid(1)-slat*sbid(3)

RETURN
END SUBROUTINE cadine84
!**********************************************************************

SUBROUTINE cadsph(blon,blat,balt,sbii,alon0)

!*** CALCULATES LON,LAT,ALT FROM INERTIAL POSITION VECTOR


REAL, INTENT(OUT)                        :: blon
REAL, INTENT(OUT)                        :: blat
REAL, INTENT(OUT)                        :: balt
REAL, INTENT(IN)                         :: sbii(3)
REAL, INTENT(IN)                         :: alon0
COMMON  c(3510)



!*** INPUT FROM EXEC

EQUIVALENCE (c(0051),rearth)
EQUIVALENCE (c(0052),crad)
EQUIVALENCE (c(0058),weii3)
EQUIVALENCE (c(2000),t)

!*** LATITUDE

dbii=SQRT(sbii(1)**2+sbii(2)**2+sbii(3)**2)
dum1=sbii(3)/dbii
blat=ASIN(dum1)
balt=dbii-rearth

!*** LONGITUDE

dum3=sbii(2)/SQRT(sbii(1)**2+sbii(2)**2)
dum4=ASIN(dum3)
IF(sbii(1) >= 0..AND.sbii(2) >= 0.) alamda=dum4
IF(sbii(1) < 0..AND.sbii(2) >= 0.) alamda=180./crad-dum4
IF(sbii(1) < 0..AND.sbii(2) < 0.) alamda=180./crad-dum4
IF(sbii(1) > 0..AND.sbii(2) < 0.) alamda=360./crad+dum4
blon=alon0+alamda-weii3*t
IF(blon > (180./crad)) blon=-((360./crad)-blon)

RETURN
END SUBROUTINE cadsph
!**********************************************************************

SUBROUTINE cadip(tip,ainc,omega,along)

!*** THIS SUBROUTINE COMPUTES THE TRANSFORMATION OF INERTIAL RELATIVE TO
!*** PERIFOCAL COORDINATE SYSTEMS


REAL, INTENT(OUT)                        :: tip(3,3)
REAL, INTENT(IN OUT)                     :: ainc
REAL, INTENT(IN OUT)                     :: omega
REAL, INTENT(IN OUT)                     :: along
COMMON c(3510)



calong=COS(along)
comega=COS(omega)
cainc=COS(ainc)
salong=SIN(along)
somega=SIN(omega)
sainc=SIN(ainc)

tip(1,1)=calong*comega-salong*somega*cainc
tip(1,2)=-calong*somega-salong*comega*cainc
tip(1,3)=salong*sainc

tip(2,1)=salong*comega+calong*somega*cainc
tip(2,2)=-salong*somega+calong*comega*cainc
tip(2,3)=-calong*sainc

tip(3,1)=somega*sainc
tip(3,2)=comega*sainc
tip(3,3)=cainc

RETURN
END SUBROUTINE cadip
!**********************************************************************

SUBROUTINE cadpt(tpt,anu,decc)

!*** THIS SUBROUTINE COMPUTES THE TRANSFORMATION OF PERIFOCAL RELATIVE
!*** TO TARGET CENTERED COORDINATE SYSTEM



REAL, INTENT(OUT)                        :: tpt(3,3)
REAL, INTENT(IN OUT)                     :: anu
REAL, INTENT(IN)                         :: decc


dum4=SIN(anu)
dum5=COS(anu)+decc
phio=ATAN2(dum4,dum5)

tpt(1,1)=-SIN(phio)
tpt(1,2)=0.
tpt(1,3)=-COS(phio)

tpt(2,1)=-tpt(1,3)
tpt(2,2)=0.
tpt(2,3)=tpt(1,1)

tpt(3,1)=0.
tpt(3,2)=-1.
tpt(3,3)=0.

RETURN
END SUBROUTINE cadpt
!**********************************************************************

SUBROUTINE cadli(tli,st1ii,vt1ii)

!*** TRANSFORMATION MATRIX OF LOCAL VERTICAL WRT. INERTIAL COOR.SYS.

!    LOCAL VERTICAL COOR.SYS.: X-> DIRECTION OF RADIUS VECTOR
!          Z-> DIRECTION OF ANGULAR MOM.VECTOR
!          Y-> COMPLETES RIGHT HANDED COOR.SYS.

REAL, INTENT(OUT)                        :: tli(3,3)
REAL, INTENT(IN)                         :: st1ii(3)
REAL, INTENT(IN)                         :: vt1ii(3)
COMMON c(3510)



CALL matabs(dt1ii,st1ii,3)
CALL matabs(dvt1ii,vt1ii,3)
al1i1=st1ii(1)/dt1ii
al1i2=st1ii(2)/dt1ii
al1i3=st1ii(3)/dt1ii
dum=dt1ii*dvt1ii
al3i1=(st1ii(2)*vt1ii(3)-st1ii(3)*vt1ii(2))/dum
al3i2=(st1ii(3)*vt1ii(1)-st1ii(1)*vt1ii(3))/dum
al3i3=(st1ii(1)*vt1ii(2)-st1ii(2)*vt1ii(1))/dum
al2i1=(al3i2*al1i3-al3i3*al1i2)
al2i2=(al3i3*al1i1-al3i1*al1i3)
al2i3=(al3i1*al1i2-al3i2*al1i1)

tli(1,1)=al1i1
tli(1,2)=al1i2
tli(1,3)=al1i3
tli(2,1)=al2i1
tli(2,2)=al2i2
tli(2,3)=al2i3
tli(3,1)=al3i1
tli(3,2)=al3i2
tli(3,3)=al3i3

RETURN
END SUBROUTINE cadli
!**********************************************************************

SUBROUTINE caddis(disbwx,blon,blat,wlon,wlat)

!*** GREAT CIRCLE GROUND DISTANCE BETWEEN B AND W IN KM: DISBWX,
!    GIVEN THE LONGITUDES AND LATITUDES OF B AND W


REAL, INTENT(OUT)                        :: disbwx
REAL, INTENT(IN OUT)                     :: blon
REAL, INTENT(IN OUT)                     :: blat
REAL, INTENT(IN OUT)                     :: wlon
REAL, INTENT(IN OUT)                     :: wlat
COMMON c(3510)

!*** INPUT FROM EXECUTIVE

EQUIVALENCE (c(0051),rearth)

dum=SIN(wlat)*SIN(blat)+COS(wlat)*COS(blat)*COS(wlon-blon)
IF(ABS(dum) > 1.)dum=SIGN(1.,dum)
disbwx=rearth*ACOS(dum)*1.e-3

RETURN
END SUBROUTINE caddis
!*******************************************************************

SUBROUTINE cadtge(tge,blon,blat)
!***  **************************************************************
!***  *
!***  * Calculates transformation matrix TGE from longitude and latitude
!***  *
!***  * Argument Output:
!***  *          TGE(3,3) =Transf. of geographic wrt earth coor
!***  * Argument Input:
!***  *          BLON =Missile longitude - rad
!***  *          BLAT =Missile latitude - rad
!***  *
!***  * MODIFICATION HISTORY
!***  * 960628 Created by Peter Zipfel
!***  *
!***  **************************************************************

REAL, INTENT(OUT)                        :: tge(3,3)
REAL, INTENT(IN OUT)                     :: blon
REAL, INTENT(IN OUT)                     :: blat
COMMON  c(3510)



slon=SIN(blon)
clon=COS(blon)
slat=SIN(blat)
clat=COS(blat)
tge(1,1)=-slat*clon
tge(1,2)=-slat*slon
tge(1,3)=clat
tge(2,1)=-slon
tge(2,2)=clon
tge(2,3)=0.
tge(3,1)=-clat*clon
tge(3,2)=-clat*slon
tge(3,3)=-slat

RETURN
END SUBROUTINE cadtge
!**********************************************************************

SUBROUTINE cadine(sbii,blon,blat,balt,alon0)

!*** CALCULATES INERTIAL POSITION VECTOR FROM LON,LAT,ALT


REAL, INTENT(OUT)                        :: sbii(3)
REAL, INTENT(IN)                         :: blon
REAL, INTENT(IN OUT)                     :: blat
REAL, INTENT(IN)                         :: balt
REAL, INTENT(IN)                         :: alon0
COMMON c(3510)



!*** INPUT FROM EXEC

EQUIVALENCE (c(0051),rearth)
EQUIVALENCE (c(0058),weii3)
EQUIVALENCE (c(2000),t)

!*** CELESTIAL LONGITUDE

blam=blon-alon0+weii3*t

!*** POSITION VECTOR

brad=rearth+balt
cblat=COS(blat)
sbii(1)=brad*cblat*COS(blam)
sbii(2)=brad*cblat*SIN(blam)
sbii(3)=brad*SIN(blat)

RETURN
END SUBROUTINE cadine
!**********************************************************************

SUBROUTINE cadtgi(tgi,alon,alat,alon0)

!*** GEOGRAPHIC TO INERTIAL COOR. TRANSFORMATION MATRIX FROM LAT AND LON


REAL, INTENT(OUT)                        :: tgi(3,3)
REAL, INTENT(IN)                         :: alon
REAL, INTENT(IN OUT)                     :: alat
REAL, INTENT(IN)                         :: alon0
COMMON c(3510)



!*** INPUT FROM EXEC

EQUIVALENCE (c(2000),t)
EQUIVALENCE (c(0058),weii3)

alamb=alon-alon0+weii3*t

tgi(1,3)=COS(alat)
tgi(3,3)=-SIN(alat)
tgi(2,2)=COS(alamb)
tgi(2,1)=-SIN(alamb)
tgi(1,1)=tgi(3,3)*tgi(2,2)
tgi(1,2)=-tgi(3,3)*tgi(2,1)
tgi(3,1)=-tgi(1,3)*tgi(2,2)
tgi(3,2)=tgi(1,3)*tgi(2,1)
tgi(2,3)=0.

RETURN
END SUBROUTINE cadtgi

!--- GETTING ENVIRONMENT VARIABLE VALUE

INTEGER(4) FUNCTION getenvqq(varname, value)
!DEC$ ATTRIBUTES DEFAULT :: GETENVQQ


, INTENT(IN OUT)                         :: 4
REAL, INTENT(IN OUT)                     :: FUNCTION g
CHARACTER (LEN=1), INTENT(IN OUT)        :: value
CHARACTER (LEN=1) :: LEN=*) varname
END FUNCTION getenvqq
